import socket 
import time
import threading
import json
import paho.mqtt.client as mqtt
from ipphone import SIPClient
import os

class GatController:
    def __init__(self, ip, port, timeout=5):
        self.ip = ip
        self.port = port
        self.timeout = timeout
        self.socket = None

    def connect(self):
        """建立TCP连接"""
        try:
            self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
            self.socket.settimeout(self.timeout)
            self.socket.connect((self.ip, self.port))
            print(f"已连接到闸机：{self.ip}:{self.port}")
            return True
        except Exception as e:
            print(f"连接失败：{e}")
            self.socket = None
            return False

    def send_command(self, hex_command):
        """发送十六进制指令"""
        if not self.socket:
            print("未建立连接，请先调用connect()")
            return False
        try:
            # 将十六进制字符串转换为字节流
            command_bytes = bytes.fromhex(hex_command)
            self.socket.sendall(command_bytes)
            print(f"已发送指令：{hex_command}")
            
            # 接收闸机响应（可选，根据闸机是否返回响应调整）
            response = self.socket.recv(1024)
            if response:
                print(f"收到响应：{response.hex().upper()}")
            return True
        except Exception as e:
            print(f"发送失败：{e}")
            return False

    def close(self):
        """关闭连接"""
        if self.socket:
            self.socket.close()
            print("连接已关闭")
        self.socket = None

def handle_client(client_socket, client_address):
    """处理客户端连接，将数据存入全局变量"""
    print(f"新连接来自: {client_address}")
    # gates = GatController("192.168.1.108",4242)
    try:
        while True:
            data = client_socket.recv(1024)
            if not data:
                print(f"客户端 {client_address} 断开连接")
                break
            # 解码数据
            decoded_data = data.decode('utf-8')
            print(f"从 {client_address} 收到: {decoded_data}")
            try:
                global devstate 
                if decoded_data == "open\n":
                    if gates.connect():
                        if gates.send_command(devList["gates"]['open']):
                            with threading.Lock():
                                devstate["gates"]["state"] = "open"
                elif decoded_data == "close\n":
                    if gates.connect():
                        if gates.send_command(devList["gates"]['close']):   
                            with threading.Lock():
                                devstate["gates"]["state"] = "close"  
                elif decoded_data == "get\n":
                    print(devstate)
            except Exception as err:
                print(err)
                print("控制闸机失败")
            # 向全局变量添加数据（加锁确保线程安全）
            global received_data
            with threading.Lock():  # 避免多线程同时修改全局变量导致冲突
                received_data.append((client_address, decoded_data))
            
            # 发送确认信息
            response = f"服务器已收到: {decoded_data}"
            client_socket.sendall(response.encode('utf-8'))
    except Exception as e:
        print(f"处理客户端 {client_address} 时出错: {e}")
    finally:
        gates.close()
        client_socket.close()

gates = GatController("192.168.1.108",4242)

def server_listen_loop(server_socket):
    """服务器监听循环（子线程中运行）"""
    try:
        while True:
            client_socket, client_address = server_socket.accept()
            client_thread = threading.Thread(
                target=handle_client,
                args=(client_socket, client_address)
            )
            client_thread.daemon = True
            client_thread.start()
    except Exception as e:
        print(f"监听线程出错: {e}")
    finally:
        server_socket.close()
        print("服务器套接字已关闭")["e"]["state"] = "close" 

def start_tcp_servce(host='192.168.1.100', port=4242):
    """启动非阻塞TCP服务器"""
    server_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)  # 允许端口复用
    try:
        server_socket.bind((host, port))
        server_socket.listen(5)
        print(f"TCP服务器已启动，监听 {host}:{port} ...")
        listen_thread = threading.Thread(
            target=server_listen_loop,
            args=(server_socket,)
        )
        listen_thread.daemon = True
        listen_thread.start()
        return listen_thread
    
    except Exception as e:
        print(f"服务器启动失败: {e}")
        server_socket.close()
        return None

class linkDev():
    def __init__(self):
        self.escalatorControl = GatController("192.168.1.107",4242)

    def setEscalator(self,state):
        if self.escalatorControl.connect():
            self.escalatorControl.send_command(devList["escalator"][state])
            return True
        return False

    def setGates(self,state):
        if gatesControl.connect():
            gatesControl.send_command(devList["gates"][state])
            return True
        return False

    def close(self):
        gatesControl.close()
        self.escalatorControl.close()
        return True


class mqttControl:
    def __init__(self,host="192.168.1.100",port=1883):
        self.dev = linkDev()

        # sip服务器设置
        self.SIP_SERVER = "192.168.1.150"    # SIP服务器IP
        self.SIP_PORT = 5060                 # SIP服务器端口
        self.USERNAME = "8001"               # 注册账号
        self.PASSWORD = "1234"               # 账号密码
        self.AUDIO_FILE = "audio.wav"        # 音频文件路径（确保存在）

        if not os.path.exists(self.AUDIO_FILE):
            print(f"❌ 错误: 音频文件不存在 - {self.AUDIO_FILE}")
            print("请修改AUDIO_FILE为正确路径（推荐16bit/单声道WAV）")
        
        # 设备信息 - 更新为闸机控制器
        self.vin = "ROBOT-SIM"  # 可以根据需要修改为ZJZK-SIM
        self.terminal_id = 3    # 闸机主控
        self.frame_id = 0
        
        
        # 初始化状态标记
        self.dt_state_initialized = False
        self.dt_men_state_initialized = False
        self.initial_close_door_executed = False

        # MQTT配置
        self.mqtt_client = mqtt.Client()
        self.mqtt_client.on_connect = self.on_mqtt_connect
        self.mqtt_client.on_message = self.on_mqtt_message

        # MQTT配置
        self.mqtt_client = mqtt.Client()
        self.mqtt_client.on_connect = self.on_mqtt_connect
        self.mqtt_client.on_message = self.on_mqtt_message
        
        # 从ROS参数获取MQTT配置
        self.mqtt_host = host
        self.mqtt_port = port

        # 连接MQTT代理
        try:
            self.mqtt_client.connect(self.mqtt_host, self.mqtt_port, 60)
            self.mqtt_client.loop_start()
        except Exception as e:
            print(f"MQTT连接失败: {e}")
        print("SlidingDoorController 已启动，提供电梯门和闸机门控制服务，支持ROS服务和MQTT控制")

        # 创建客户端并执行呼叫
        self.client = SIPClient(
            sip_server=self.SIP_SERVER,
            sip_port=self.SIP_PORT,
            username=self.USERNAME,
            password=self.PASSWORD,
            audio_file=self.AUDIO_FILE
        )

    def on_mqtt_connect(self, client, userdata, flags, rc):
        """MQTT连接回调"""
        if rc == 0:
            print("MQTT连接成功")
            # 订阅电梯任务主题和闸机任务主题
            client.subscribe("dt/task_manager")
            # client.subscribe("zj/task_manager")
            print("已订阅主题: dt/task_manager, zj/task_manager")
        else:
            print(f"MQTT连接失败，错误码: {rc}")

    def on_mqtt_message(self, client, userdata, msg):
        """MQTT消息回调"""
        try:
            payload = json.loads(msg.payload.decode())
            print(f"收到MQTT消息: {payload}")
            
            # 检查消息格式
            if not self.validate_message(payload):
                return
            
            if payload.get("VIN") == "ROBOT-SIM":
                return
            
            # 处理电梯控制指令 (sys_data_type = 1006)
            if payload.get("sys_data_type") == 1006:
                self.handle_elevator_control(payload)
            
            # 处理闸机控制指令 (sys_data_type = 1005)
            elif payload.get("sys_data_type") == 1005:
                self.handle_gate_control(payload)

            elif payload.get("sys_data_type") == 1007:
                self.handle_callphone_control(payload)
                
        except json.JSONDecodeError as e:
            print(f"JSON解析错误: {e}")
        except Exception as e:
            print(f"处理MQTT消息时发生错误: {e}")

    def validate_message(self, payload):
        """验证消息格式"""
        required_fields = ["VIN", "terminal_id", "frame_id", "sys_data_type", "time_stamp", "code", "data"]
        for field in required_fields:
            if field not in payload:
                print(f"消息缺少必要字段: {field}")
                return False
        return True
    

    def handle_elevator_control(self, payload):
        """处理电梯控制指令"""
        try:
            data = payload.get("data", {})
            task_type = data.get("tasktype")
            task_id = data.get("task_id")
            
            response_data = {
                "task_status": 1,  # 1=接收任务
                "error_code": 0,
                "task_id": task_id
            }
            
            if task_type == 1:  # 开门
                print(f"收到开门指令，任务ID: {task_id}")
                # 在单独线程中执行开门操作，避免阻塞MQTT
                threading.Thread(target=self._execute_open_door, args=(task_id,)).start()
            elif task_type == 2:  # 关门
                print(f"收到关门指令，任务ID: {task_id}")
                # 在单独线程中执行关门操作，避免阻塞MQTT
                threading.Thread(target=self._execute_close_door, args=(task_id,)).start()
            elif task_type == 3:  # 呼叫电梯
                threading.Thread(target=self._call_elecator, args=(task_id,data.get("current_floor"))).start()
            elif task_type == 4:  # 按电梯
                threading.Thread(target=self._press_elecator, args=(task_id,data.get("target_floor"))).start()
            else:
                print(f"未知的任务类型: {task_type}")
                response_data["task_status"] = 3  # 任务失败
                response_data["error_code"] = 1
            
            # 发送任务接收确认
            self.send_mqtt_response(payload, response_data)
            
        except Exception as e:
            print(f"处理电梯控制指令时发生错误: {e}")
            response_data = {
                "task_status": 3,  # 任务失败
                "error_code": 1,
                "task_id": data.get("task_id", 0)
            }
            self.send_mqtt_response(payload, response_data)

    def handle_callphone_control(self, payload):
        "处理电话控制命令"
        try:
            data = payload.get("data", {})
            task_type = data.get("tasktype")
            point_id = data.get("point_id")
            print("开始电话控制指令")
            self._execute_call_phone(point_id)
            print("处理电话控制指令成功")
            response_data = {
                "task_status": 1,  # 任务失败
                "error_code": 1,
                "task_id": data.get("task_id", 0)
            }
        except Exception as e:
            print(f"处理电话控制指令时发生错误: {e}")
            response_data = {
                "task_status": 3,  # 任务失败
                "error_code": 1,
                "task_id": data.get("task_id", 0)
            }
        self.send_mqtt_response(payload, response_data)

    def handle_gate_control(self, payload):
        """处理闸机控制指令"""
        try:
            data = payload.get("data", {})
            task_type = data.get("tasktype")
            task_id = data.get("task_id")
            
            response_data = {
                "task_status": 1,  # 1=接收任务
                "error_code": 0,
                "task_id": task_id
            }
            
            if task_type == 1:  # 开闸机门
                print(f"收到开闸机门指令，任务ID: {task_id}")
                # 在单独线程中执行开门操作，避免阻塞MQTT
                threading.Thread(target=self._execute_open_gate, args=(task_id,)).start()
                
            elif task_type == 2:  # 关闸机门
                print(f"收到关闸机门指令，任务ID: {task_id}")
                # 在单独线程中执行关门操作，避免阻塞MQTT
                threading.Thread(target=self._execute_close_gate, args=(task_id,)).start()
                
            else:
                print(f"未知的闸机任务类型: {task_type}")
                response_data["task_status"] = 3  # 任务失败
                response_data["error_code"] = 1
            
            # 发送任务接收确认
            self.send_mqtt_response(payload, response_data)
        except Exception as e:
            print(f"处理闸机控制指令时发生错误: {e}")
            response_data = {
                "task_status": 3,  # 任务失败
                "error_code": 1,
                "task_id": data.get("task_id", 0)
            }
            self.send_mqtt_response(payload, response_data)

    def _execute_call_phone(self,point_id):
        """执行初始电话操作"""
        try:
            print(f"正在拨打房间：{point_id}电话")
            phoneNum = {"201":8000,"202":8002,"301":8003,"302":8004}
            self.client.start_call_sequence(phoneNum[point_id])
            response_data = {
                "task_status": 2,  # 任务成功
                "error_code": 0,
                "task_id": point_id
            }
            print(f"房间：{point_id}电话已经挂断")
        except Exception as e:
            print(f"拨打房间：{point_id}电话失败: {e}")
            response_data = {
                "task_status": 2,  # 任务失败
                "error_code": 0,
                "task_id": point_id
            }
        time.sleep(1)
        self.send_task_completion_response(point_id, response_data)

    def _execute_open_door(self,task_id):
        """执行初始开门操作"""
        try:
            if self.dev.setEscalator('open'):
                global devstate 
                with threading.Lock():
                    devstate["escalator"]["state"] = "work"
                    devstate["escalator"]["detall"] = "open"
                time.sleep(7)
                # 发送任务成功响应
                response_data = {
                    "task_status": 2,  # 任务成功
                    "error_code": 0,
                    "task_id": task_id
                }
                print("门已开启")
            else:
                response_data = {
                    "task_status": 3,  # 任务失败
                    "error_code": 0,
                    "task_id": task_id
                }
            time.sleep(8)
            self.send_task_completion_response(task_id, response_data)
        except Exception as e:
            print(f"初始开门操作失败: {e}")
            response_data = {
                "task_status": 3,  # 任务失败
                "error_code": 0,
                "task_id": task_id
            }
            self.send_task_completion_response(task_id, response_data)

    def _execute_close_door(self,task_id):
        """执行初始关门操作"""
        try:
            if self.dev.setEscalator('close'):
                global devstate 
                with threading.Lock():
                    devstate["escalator"]["state"] = "work"
                    devstate["escalator"]["detall"] = "close"
                # 发送任务成功响应
                response_data = {
                    "task_status": 2,  # 任务成功
                    "error_code": 0,
                    "task_id": task_id
                }
                print("门已关闭")
            else:
                response_data = {
                    "task_status": 3,  # 任务失败
                    "error_code": 0,
                    "task_id": task_id
                }
            time.sleep(8)
            self.send_task_completion_response(task_id, response_data)
        except Exception as e:
            print(f"初始关门操作失败: {e}")
            response_data = {
                "task_status": 3,  # 任务失败
                "error_code": 0,
                "task_id": task_id
            }
            self.send_task_completion_response(task_id, response_data)

    def _call_elecator(self, task_id, floor):
        """呼叫电梯"""
        try:
            # 呼叫电梯 延时2s
            print(f"呼叫电梯 到{floor}层 ")
            time.sleep(2)
            with threading.Lock():  # 避免多线程同时修改全局变量导致冲突
                received_data.append(('192.168.1.100', str(floor)))
            print("呼叫电梯操作成功")
            response_data = {
                "task_status": 2,  # 任务成功
                "error_code": 0,
                "task_id": task_id
            }
            self.send_task_completion_response(task_id, response_data)
        except Exception as e:
            print(f"呼叫电梯操作失败: {e}")
            response_data = {
                "task_status": 3,  # 任务失败
                "error_code": 1,
                "task_id": task_id
            }
            self.send_task_completion_response(task_id, response_data)

    def _press_elecator(self, task_id, floor):
        """按电梯"""
        try:
            # 按电梯 延时2s
            print(f"按电梯 到{floor}层 ")
            time.sleep(2)
            # 发送任务成功响应
            with threading.Lock():  # 避免多线程同时修改全局变量导致冲突
                received_data.append(('192.168.1.100', str(floor)))
            print("按电梯成功")
            response_data = {
                "task_status": 2,  # 任务成功
                "error_code": 0,
                "task_id": task_id
            }
            self.send_task_completion_response(task_id, response_data)
        except Exception as e:
            print(f"按电梯操作失败: {e}")
            response_data = {
                "task_status": 3,  # 任务失败
                "error_code": 1,
                "task_id": task_id
            }
            self.send_task_completion_response(task_id, response_data)

    def send_mqtt_response(self, original_payload, response_data):
        """发送MQTT响应"""
        self.frame_id += 1
        response = {
            "VIN": self.vin,
            "terminal_id": self.terminal_id,
            "frame_id": self.frame_id,
            "sys_data_type": original_payload.get("sys_data_type"),
            "time_stamp": int(time.time() * 1000),
            "code": 0,  # 响应成功
            "data": response_data
        }
        
        try:
            # 根据消息类型选择正确的主题
            sys_data_type = original_payload.get("sys_data_type")
            # if sys_data_type == 1005:  # 闸机控制指令
            #     topic = "zj/task_manager"
            # elif sys_data_type == 1006:  # 电梯控制指令
            #     topic = "dt/task_manager"
            # else:
            topic = "dt/task_manager"  # 默认主题
                
            self.mqtt_client.publish(topic, json.dumps(response))
            print(f"已发送MQTT响应到主题 {topic}: {response}")
        except Exception as e:
            print(f"发送MQTT响应失败: {e}")

    def send_task_completion_response(self, task_id, response_data):
        """发送任务完成响应"""
        self.frame_id += 1
        response = {
            "VIN": self.vin,
            "terminal_id": self.terminal_id,
            "frame_id": self.frame_id,
            "sys_data_type": 1006,  # 电梯控制指令
            "time_stamp": int(time.time() * 1000),
            "code": 0,  # 响应成功
            "data": response_data
        }
        try:
            topic = "dt/task_manager"
            self.mqtt_client.publish(topic, json.dumps(response))
            print(f"已发送任务完成响应: {response}")
        except Exception as e:
            print(f"发送任务完成响应失败: {e}")

    def handle_door_control(self, req):
        """处理电梯门控制请求"""
        try:
            print(f"处理闸机门控制请求: mode={req.mode}")
            mode = req.mode
            
            if mode == 1:  # 开门
                with threading.Lock():  # 避免多线程同时修改全局变量导致冲突
                    received_data.append(('192.168.1.100', "open"))
                time.sleep(3)
                message = "电梯门已打开"
            elif mode == 2:  # 开另一侧门
                with threading.Lock():  # 避免多线程同时修改全局变量导致冲突
                    received_data.append(('192.168.1.100', "close"))
                time.sleep(3)
                message = "电梯门已关闭"
            # 等待命令生效
            time.sleep(0.1)
            print(message)
            
            # 返回响应
            return True
        except Exception as e:
            return False
        
    def _execute_open_gate(self, task_id):
        """执行开闸机门操作"""
        try:
            print("正在开闸机门...")
            # 调用现有的handle_door_control方法，mode=1表示开门
            # fake_req = type('obj', (object,), {'mode': 1})()
            global devstate 
            if gates.connect():
                if gates.send_command(devList["gates"]['open']):   
                    with threading.Lock():
                        devstate["gates"]["state"] = "open"  
                time.sleep(3)
                print("闸机门已打开")
                # 发送任务成功响应
                response_data = {
                    "task_status": 2,  # 任务成功
                    "error_code": 0,
                    "task_id": task_id
                }
            else:
                print("闸机门打开失败")
                # 发送任务成功响应
                response_data = {
                    "task_status": 3,  # 任务失败
                    "error_code": 0,
                    "task_id": task_id
                }
        except Exception as e:
            print(f"开闸机门操作失败: {e}")
            response_data = {
                "task_status": 3,  # 任务失败
                "error_code": 1,
                "task_id": task_id
            }
        self.send_gate_completion_response(task_id, response_data)

    def _execute_close_gate(self, task_id):
        """执行关闸机门操作"""
        try:
            print("正在关闸机门...")
            # 调用现有的handle_door_control方法，mode=0表示关门
            # fake_req = type('obj', (object,), {'mode': 0})()
            global devstate 
            if gates.connect():
                if gates.send_command(devList["gates"]['close']):   
                    with threading.Lock():
                        devstate["gates"]["state"] = "close"  
                time.sleep(3)
                print("闸机门已关闭")
                # 发送任务成功响应
                response_data = {
                    "task_status": 2,  # 任务成功
                    "error_code": 0,
                    "task_id": task_id
                }
            else:
                print("闸机门关闭失败")
                # 发送任务成功响应
                response_data = {
                    "task_status": 3,  # 任务失败
                    "error_code": 0,
                    "task_id": task_id
                }
            
        except Exception as e:
            print(f"关闸机门操作失败: {e}")
            response_data = {
                "task_status": 3,  # 任务失败
                "error_code": 1,
                "task_id": task_id
            }
        self.send_gate_completion_response(task_id, response_data)

    def send_gate_completion_response(self, task_id, response_data):
        """发送闸机任务完成响应"""
        self.frame_id += 1
        response = {
            "VIN": self.vin,
            "terminal_id": self.terminal_id,
            "frame_id": self.frame_id,
            "sys_data_type": 1005,  # 闸机控制指令
            "time_stamp": int(time.time() * 1000),
            "code": 0,  # 响应成功
            "data": response_data
        }
        
        try:
            topic = "dt/task_manager"
            self.mqtt_client.publish(topic, json.dumps(response))
            print(f"已发送闸机任务完成响应: {response}")
        except Exception as e:
            print(f"发送闸机任务完成响应失败: {e}")

    def shutdown(self):
        """关闭MQTT连接"""
        try:
            self.mqtt_client.loop_stop()
            self.mqtt_client.disconnect()
            self.client.cleanup(force=True)
            print("MQTT连接已断开")
        except Exception as e:
            print(f"断开MQTT连接时发生错误: {e}")
        

class Escalator:
    def __init__(self,start_floor=1):
        self.escalator_thread = None
        self.start_floor = start_floor
        self.floor_requests = []
        self.dev = linkDev()
        self.running_flag = False

    def setDisplay(self,state):
        if self.dev.setEscalator(state):
            global devstate 
            if state in ["up","down","none"]:
                with threading.Lock():
                    devstate["escalator"]["state"] = "work"
                    devstate["escalator"]["detall"] = state
            else:
                try:
                    with threading.Lock():
                        devstate["escalator"]["state"] = "work"
                        devstate["escalator"]["floor"] = int(state)
                except ValueError as err:
                    print(err)
            time.sleep(3)

    def running(self):
        self.running_flag = True  # 启动时设置为True
        self.setDisplay('1')
        while(self.running_flag):
            try:
                time.sleep(0.5)
                # 获取电梯请求数据
                global received_data
                with threading.Lock():
                    for addr, data in received_data:
                        try:
                            if not int(data) in self.floor_requests:
                                if int(data) in floor:
                                    self.floor_requests.append(int(data))
                                    print(self.floor_requests)
                                    print('已接收到电梯请求数据')
                        except ValueError as err:
                            print(err)
                            # print("未收到请求数据")
                    received_data = []
                # 处理电梯请求
                num = len(self.floor_requests)
                for i in range(num):
                    self.up_floor = self.floor_requests[0]
                    if self.up_floor > self.start_floor:
                        self.setDisplay('up')
                    elif self.up_floor < self.start_floor:
                        self.setDisplay('down')
                    self.setDisplay('none')
                    self.start_floor = self.up_floor
                    self.setDisplay(str(self.start_floor))
                    del self.floor_requests[0]
                global devstate 
                with threading.Lock():
                    devstate["escalator"]["state"] = "IDLE"
                    devstate["escalator"]["detall"] = "none"
            except Exception as err:
                print(err)
        print("电梯控制器线程已退出")

    def start(self):
        try:
            if self.escalator_thread and self.escalator_thread.is_alive():
                print("电梯控制器线程已在运行")
                return self.escalator_thread
            print(f"电梯控制器已打开")
            self.escalator_thread = threading.Thread(
                target=self.running,
                args=(),
                name="EscalatorControllerThread"
            )
            self.escalator_thread.daemon = True
            self.escalator_thread.start()
            return self.escalator_thread
        except Exception as e:
            print(f"电梯控制器启动失败: {e}")
            return None

    def stop(self):
        """停止线程（协作式退出）"""
        gates.close()
        client_socket.close()
        if not self.escalator_thread or not self.escalator_thread.is_alive():
            print("电梯控制器线程未在运行")
            return
        
        # 设置标志位为False，通知线程退出
        self.running_flag = False
        # 等待线程结束（最多等待5秒，避免无限阻塞）
        self.escalator_thread.join(timeout=5)
        if self.escalator_thread.is_alive():
            print("警告：线程未正常退出，可能仍在执行收尾操作")
        else:
            print("电梯控制器已关闭")
            self.escalator_thread = None  # 清空线程对象

if __name__ == "__main__":
    try:
        esc = Escalator()
        # 启动tcp服务器
        start_tcp_servce()
        # 启动电梯控制器
        esc.start()
        mt = mqttControl()
        while(1):
            time.sleep(1)

    finally:
        mt.shutdown()
        gates.close()
        esc.stop()
        



    
        
